from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    pkg_name = 'rms_pkg'
    pkg_share_dir = get_package_share_directory(pkg_name)
    config_dir = os.path.join(pkg_share_dir, 'config')
    params_file = os.path.join(config_dir, 'init_params.yaml')

    # 声明 robot_id 参数
    robot_id_arg = DeclareLaunchArgument(
        'robot_id',
        default_value='robot1001',
        description='Robot ID to use as namespace'
    )

    # 获取 robot_id 的值
    robot_id = LaunchConfiguration('robot_id')

    heartbeat_node = Node(
        package=pkg_name,
        executable='heartbeat_publisher',
        name='heartbeat_publisher',
        parameters=[params_file],
        output='log',
        arguments=['--log-level', 'DEBUG'],
    )

    sensor_m01h20_node = Node(
        package=pkg_name,
        executable='sensor_m01h20_publisher',
        name='sensor_m01h20_publisher',
        parameters=[params_file],
        output='log',
        arguments=['--log-level', 'DEBUG'],
        namespace=robot_id
    )

    sensor_hcd5014_node = Node(
        package=pkg_name,
        executable='sensor_hcd5014_publisher',
        name='sensor_hcd5014_publisher',
        parameters=[params_file],
        output='log',
        arguments=['--log-level', 'DEBUG'],
        namespace=robot_id
    )

     # 添加机器人状态服务节点
    robot_status_server_node = Node(
        package=pkg_name,
        executable='robot_status_server',
        name='robot_status_server',
        parameters=[params_file],
        output='log',
        arguments=['--log-level', 'DEBUG'],
        namespace=robot_id
    )

    # 添加任务服务Action节点
    task_action_server = Node(
        package=pkg_name,
        executable='task_action_server',
        name='task_action_server',
        parameters=[params_file],
        output='screen',
        arguments=['--log-level', 'DEBUG'],
        namespace=robot_id
    )
    
    # 添加地图下载Action服务器节点
    map_download_action_server_node = Node(
        package=pkg_name,
        executable='map_download_action_server',
        name='map_download_action_server',
        parameters=[params_file],
        output='log',
        arguments=['--log-level', 'INFO'],
        namespace=robot_id
    )

    # 添加日志服务器节点
    robot_tcp_server_node = Node(
        package=pkg_name,
        executable='robot_tcp_server',
        name='robot_tcp_server',
        parameters=[params_file],
        output='log',
        arguments=['--log-level', 'DEBUG'],
        namespace=robot_id
    )

    return LaunchDescription([
        robot_id_arg,
        heartbeat_node,
        sensor_m01h20_node,
        #sensor_hcd5014_node,
        robot_status_server_node,
        task_action_server,
        #map_download_action_server_node,
        robot_tcp_server_node
    ])